#include "task_gimbal.h"
#include "app.hpp"

static uint32_t dwt_cnt;
static float dt;

static DJ6::switch_t last_right_switch = DJ6::ERR;
static DJ6::switch_t last_left_switch = DJ6::ERR;
static bool last_mouse_left = false;
static int16_t last_mouse_z_dir = 0;
static bool last_x = false;

// 解析遥控器操作
static void handle_rc() {
    // 1. 云台pitch、yaw电机
    // 遥控和键盘叠加控制
    status.gimbal.rc.pitch_speed = dj6.pitch * settings.pitch_aps_max;
    status.gimbal.rc.yaw_speed = -dj6.yaw * settings.yaw_aps_max;

    // 2. 右拨杆控制云台模式
    // 只响应变化量
    if (dj6.right_switch != last_right_switch) {
        last_right_switch = dj6.right_switch;
        if (dj6.right_switch == DJ6::UP) {
            status.gimbal.mode = Gimbal::ECD_MODE; // UP为CDO模式
        } else {
            status.gimbal.mode = Gimbal::IMU_MODE; // MID、DOWN为IMU模式
        }
    }

    // 3. 左摇杆控制发射机构
    // 只响应变化量
    if (dj6.left_switch != last_left_switch) {
        last_left_switch = dj6.left_switch;
        if (dj6.left_switch == DJ6::UP) {
            // 摩擦轮关，拨弹电机关
            status.gimbal.is_prepare_shoot = false;
            status.gimbal.is_shoot = false;
        } else if (dj6.left_switch == DJ6::MID) {
            // 摩擦轮开，拨弹电机关
            status.gimbal.is_prepare_shoot = true;
            status.gimbal.is_shoot = false;
        } else if (dj6.left_switch == DJ6::DOWN) {
            // 摩擦轮开，拨弹电机开
            status.gimbal.is_prepare_shoot = true;
            status.gimbal.is_shoot = true;
        }
    }
}

// 解析鼠标操作
static void handle_video() {
    // 1. 云台pitch、yaw电机
    // 遥控和键盘叠加控制
    status.gimbal.video.pitch_speed = -static_cast<float>(referee.mouse_y) / settings.mouse_y_max * settings.
                                      pitch_aps_max;
    status.gimbal.video.yaw_speed = -static_cast<float>(referee.mouse_x) / settings.mouse_x_max * settings.yaw_aps_max;

    // 2. 左键开火
    // 只响应变化量
    if (referee.mouse_left_button_down != last_mouse_left) {
        last_mouse_left = referee.mouse_left_button_down;
        status.gimbal.is_shoot = referee.mouse_left_button_down;
    }

    // 3. 右键自瞄
    // 仅在云台IMU模式下允许自瞄
    if (referee.mouse_right_button_down && status.gimbal.mode == Gimbal::IMU_MODE) {
        status.gimbal.is_rv2_mode = true;
    } else {
        status.gimbal.is_rv2_mode = false;
    }

    // 4. 鼠标滚轮射频调节
    int16_t now_mouse_z_dir;
    if (referee.mouse_z == 0) {
        now_mouse_z_dir = 0;
    } else if (referee.mouse_z > 0) {
        now_mouse_z_dir = 1;
    } else {
        now_mouse_z_dir = -1;
    }
    if (now_mouse_z_dir != last_mouse_z_dir) {
        last_mouse_z_dir = now_mouse_z_dir;
        if (last_mouse_z_dir > 0) {
            status.gimbal.shoot_freq += settings.shoot_freq_per_press;
        } else if (last_mouse_z_dir < 0) {
            status.gimbal.shoot_freq -= settings.shoot_freq_per_press;
        }
        status.gimbal.shoot_freq = clamp(status.gimbal.shoot_freq, settings.shoot_freq_min, settings.shoot_freq_max);
    }

    // 5. 按X反转拨弹电机，用于改善摩擦轮启动失败
    if (referee.x != last_x) {
        last_x = referee.x;
        if (referee.x) {
            status.gimbal.shoot_freq = settings.backward_shoot_freq;
        } else {
            status.gimbal.shoot_freq= settings.shoot_freq_min;
        }
    }
}

static bool checkHeatProtect() {
    // if (referee.shooter_17mm_heat > referee.shooter_heat_limit * 0.1) {
    //     return false;
    // } else {
    //     return true;
    // }
    return true;
}

[[noreturn]] void task_gimbal_entry(void const *argument) {
    imu.WaitReady();
    gimbal.WaitReady();

    while (true) {
        dt = BSP_DWT_GetDeltaT(&dwt_cnt);

        handle_rc();
        handle_video();
        if (referee.competition_is_started == 1) {
            status.gimbal.is_prepare_shoot = true;
        }

        // 检查遥控器连接 是否为强制键盘模式
        if (dj6.is_connected == false && not status.ignore_rc_disconnect) {
            gimbal.SetEnable(false); // 云台失能，关闭所有电机输出
            osDelay(1);
            continue;
        }

        // 控制云台
        gimbal.SetEnable(true); // 使能
        gimbal.SetMode(status.gimbal.mode); // 模式
        gimbal.SetYawSpeedFF(-chassis.measure.chassis.vr); // yaw速度前馈
        gimbal.SetPrepareShoot(status.gimbal.is_prepare_shoot); // 摩擦轮

        // 自瞄模式
        if (status.gimbal.is_rv2_mode) {
            if (rv2.is_locked) {
                gimbal.SetAngle(rv2.pitch, rv2.yaw);
            }
            // rv2发送开火建议，且选手允许开火才开火
            if (status.gimbal.is_shoot && checkHeatProtect()) {
                gimbal.SetShoot(true, status.gimbal.shoot_freq);
            } else {
                gimbal.SetShoot(false);
            }
        } else {
            // 手动模式
            // 合并遥控器和键鼠控制
            const float pitch_angle_add = (status.gimbal.rc.pitch_speed + status.gimbal.video.pitch_speed) * dt;
            const float yaw_angle_add = (status.gimbal.rc.yaw_speed + status.gimbal.video.yaw_speed) * dt;
            gimbal.AddAngle(pitch_angle_add, yaw_angle_add); // 角度增量
            if (checkHeatProtect()) {
                gimbal.SetShoot(status.gimbal.is_shoot, status.gimbal.shoot_freq); // 是否开火
            } else {
                gimbal.SetShoot(false);
            }
        }

        gimbal.Update();
        osDelay(1);
    }
}

void task_gimbal_callback(const uint32_t id, uint8_t data[8]) {
    gimbal.ParseCAN(id, data);
}
